#include "memory.hpp"
#include "helper.hpp"

using namespace qOS;


static const size_t BYTE_ALIGN_MASK = static_cast<size_t>(Q_BYTE_ALIGNMENT) - static_cast<size_t>(1U);
static const size_t ALLOC_BIT_SEL = (sizeof(size_t)*static_cast<size_t>(8U)) - static_cast<size_t>(1U);
static const size_t BLOCK_ALLOCATED_BIT = static_cast<size_t>(1U) << ALLOC_BIT_SEL;
static const size_t HEAP_STRUCT_SIZE = (sizeof(mem::blockConnect_t) +
                                       (BYTE_ALIGN_MASK - static_cast<size_t>(1U))) & ~BYTE_ALIGN_MASK;

//==============================================================================
bool mem::pool::setup(void *pArea, const size_t pSize) noexcept
{
    bool retValue = false;

    if ((nullptr != pArea) && (pSize > 0U)) {
        /*cstat -CERT-EXP36-C_b*/
        poolMemory = static_cast<uint8_t*>(pArea);
        /*cstat +CERT-EXP36-C_b*/
        poolMemSize = pSize;
        freeBytesRemaining = pSize;
        end = nullptr;
        init();
        retValue = true;
    }

    return retValue;
}
//==============================================================================
void mem::pool::insertBlockIntoFreeList(blockConnect_t *xBlock) noexcept
{
    mem::blockConnect_t *iterator;
    uint8_t *ptr;

    for (iterator = &start;  iterator->next < xBlock;  iterator = iterator->next) {}
    ptr = reinterpret_cast<uint8_t*>(iterator);
    /*cstat -SEC-NULL-cmp-bef -PTR-null-cmp-bef -CERT-EXP34-C_g*/
    if (&ptr[iterator->blockSize] == reinterpret_cast<uint8_t*>(xBlock)) {
        iterator->blockSize += xBlock->blockSize;
        xBlock = iterator;
    }
    ptr = reinterpret_cast<uint8_t*>(xBlock);
    if (&ptr[xBlock->blockSize] == reinterpret_cast<uint8_t*>(iterator->next)) {
        if (iterator->next != end) {
            xBlock->blockSize += iterator->next->blockSize;
            xBlock->next = iterator->next->next;
        }
        else {
            xBlock->next = end;
        }
    }
    else {
        xBlock->next = iterator->next;
    }
    /*cstat +SEC-NULL-cmp-bef +PTR-null-cmp-bef +CERT-EXP34-C_g*/
    if (iterator != xBlock) {
        iterator->next = xBlock;
    }
}
//==============================================================================
void mem::pool::free(void *ptr) noexcept
{
    /*cstat -CERT-EXP36-C_b*/
    uint8_t *pToFree = static_cast<uint8_t*>(ptr);
    /*cstat +CERT-EXP36-C_b*/
    if (nullptr != ptr) {
        mem::blockConnect_t *xConnect;

        pToFree -= HEAP_STRUCT_SIZE;
        xConnect = aligned_cast<mem::blockConnect_t*>(pToFree);
        if (0U != (xConnect->blockSize & BLOCK_ALLOCATED_BIT)) {
            if (NULL == xConnect->next) {
                bits::multipleClear(xConnect->blockSize, BLOCK_ALLOCATED_BIT);
                freeBytesRemaining += xConnect->blockSize;
                insertBlockIntoFreeList(xConnect);
            }
        }
    }
}
//==============================================================================
void mem::pool::init(void) noexcept
{
    mem::blockConnect_t *firstFreeBlock;
    uint8_t *aligned;
    mem::address_t address;
    mem::address_t xAddrTmp;
    size_t totalPoolSize = poolMemSize;

    /*cstat -CERT-INT36-C -CERT-EXP39-C_d -CERT-EXP36-C_a*/
    address = reinterpret_cast<mem::address_t>(poolMemory);

    if (0UL != (address & BYTE_ALIGN_MASK)) {
        address += BYTE_ALIGN_MASK;
        address &= ~BYTE_ALIGN_MASK;
        totalPoolSize -= address - reinterpret_cast<mem::address_t>(poolMemory);
    }

    aligned = reinterpret_cast<uint8_t*>(address);
    start.next = aligned_cast<mem::blockConnect_t*>(aligned);
    start.blockSize = static_cast<size_t>(0U);
    xAddrTmp = reinterpret_cast<address_t>(aligned);
    address = xAddrTmp + totalPoolSize;
    address -= HEAP_STRUCT_SIZE;
    address &= ~BYTE_ALIGN_MASK;
    end = reinterpret_cast<mem::blockConnect_t*>(address);
    end->next = nullptr;
    end->blockSize = static_cast<size_t>(0U);
    firstFreeBlock = aligned_cast<mem::blockConnect_t*>(aligned);
    xAddrTmp = reinterpret_cast<mem::address_t>(firstFreeBlock);
    /*cstat +CERT-INT36-C +CERT-EXP39-C_d +CERT-EXP36-C_a*/
    firstFreeBlock->blockSize = address - xAddrTmp;
    firstFreeBlock->next = end;
    freeBytesRemaining = firstFreeBlock->blockSize;
}
//==============================================================================
void* mem::pool::alloc(size_t pSize) noexcept
{
    void *pAllocated = nullptr;

    if (pSize > static_cast<size_t>(0U)) {
        const size_t additional = HEAP_STRUCT_SIZE + Q_BYTE_ALIGNMENT - (pSize & BYTE_ALIGN_MASK);
        if (pSize > ((~static_cast<size_t>(0U)) - additional)) {
            pSize = static_cast<size_t>(0U);
        }
        else {
            /*cstat -ATH-overflow*/
            pSize += additional;
            /*cstat +ATH-overflow*/
        }
    }

    if (static_cast<size_t>(0U) == (pSize & BLOCK_ALLOCATED_BIT)) {
        if ((pSize > static_cast<size_t>(0U)) && (pSize < freeBytesRemaining)) {
            mem::blockConnect_t *xBlock;
            mem::blockConnect_t *previousBlock;
    
            previousBlock = &start;
            xBlock = start.next;
            while ((xBlock->blockSize < pSize) && (nullptr != xBlock->next)) {
                previousBlock = xBlock;
                xBlock = xBlock->next;
            }
            if (xBlock != end) {
                const size_t minBlockSize = (HEAP_STRUCT_SIZE << static_cast<size_t>(1U));

                pAllocated = static_cast<void*>((reinterpret_cast<uint8_t*>(previousBlock->next)) + HEAP_STRUCT_SIZE);
                previousBlock->next = xBlock->next;
                if ((xBlock->blockSize - pSize) > minBlockSize) {
                    mem::blockConnect_t *newBlockLink;
                    /*cstat -MISRAC++2008-7-1-1*/
                    uint8_t *pBlockU8 = reinterpret_cast<uint8_t*>(xBlock);
                    uint8_t *ptrBlock = &pBlockU8[pSize];
                    /*cstat -CERT-EXP39-C_d -CERT-EXP36-C_a +MISRAC++2008-7-1-1*/
                    newBlockLink = aligned_cast<mem::blockConnect_t*>(ptrBlock);
                    /*cstat +CERT-EXP39-C_d +CERT-EXP36-C_a*/
                    newBlockLink->blockSize = xBlock->blockSize - pSize;
                    /*cstat -ATH-overflow*/
                    xBlock->blockSize = pSize;
                    /*cstat +ATH-overflow*/
                    insertBlockIntoFreeList(newBlockLink);
                }
                freeBytesRemaining -= xBlock->blockSize;
                bits::multipleSet(xBlock->blockSize, BLOCK_ALLOCATED_BIT);
                xBlock->next = nullptr;
            }
        }
    }

    return pAllocated;
}
//==============================================================================
size_t mem::pool::getFreeSize(void) const noexcept
{
    size_t retValue = poolMemSize;

    if (nullptr != end) {
        retValue = freeBytesRemaining;
    }

    return retValue;
}
//==============================================================================

#if (Q_DEFAULT_HEAP_SIZE >= 64)

static uint8_t defaultPoolMemory[Q_DEFAULT_HEAP_SIZE] = { 0 };
static mem::pool defaultMemPool(defaultPoolMemory, Q_DEFAULT_HEAP_SIZE);

//==============================================================================
void * operator new(size_t size)
{
    return defaultMemPool.alloc(size);
}
//==============================================================================
void * operator new[](size_t size)
{
    return operator new(size);
}
//==============================================================================
/*cstat -MISRAC++2008-7-1-2 -MISRAC++2008-0-1-11 -CPU-delete-void*/
void * operator new(size_t size, void *place) noexcept
{
    /* Nothing to do */
    (void)size;
    return place;
}
//==============================================================================
void * operator new[](size_t size, void *place) noexcept
{
    return operator new(size, place);
}
//==============================================================================
void operator delete(void * ptr) noexcept
{
    defaultMemPool.free(ptr);
}
//==============================================================================
void operator delete[](void * ptr) noexcept
{
    operator delete(ptr);
}
//==============================================================================
void operator delete(void* ptr, void* place) noexcept
{
    (void)ptr;
    (void)place;
    /*Nothing to do*/
}
//==============================================================================
void operator delete[](void* ptr, void* place) noexcept
{
    (void)ptr;
    (void)place;  // unused
    /*Nothing to do*/
}
/*cstat +MISRAC++2008-7-1-2 +MISRAC++2008-0-1-11 +CPU-delete-void*/
//==============================================================================
#endif /*Q_USE_MEM_ALLOCATION_SCHEME*/
